#ifndef __CUDA_COLLISION_H_
#define __CUDA_COLLISION_H_

#include "cuda_robobs.h"
#include "cuda_defs.h"
#include "defs.h"
#include "cuda_transform_util.h"


/// Note: collideSamplesXXXXX functions return pairs (a, b). (a == -1, b == -1) --> no collision. Otherwise, return the collide pair index.
template <class StackElem, int StackFullDefault, class CollideFunctor>
__global__ void collideSamples3T3R(GRobot* robot, GObstacles* obs, float* samples, unsigned int nSamples, unsigned int* collide_results, CollideFunctor collideF)
{
	unsigned int threadId = (blockIdx.y * gridDim.x + blockIdx.x) * (blockDim.x * blockDim.y) + threadIdx.y * blockDim.x + threadIdx.x;
	if(threadId >= nSamples)
		return;
		
	GTransform t;
	
	t.ToSE3(samples + threadId, nSamples);
	
	for(unsigned int j = 0; j < obs->n; ++j)
	{
		if(collideF(robot->models[0], robot->vertexPointers[0], robot->triIdxPointers[0], t, obs->models[j], obs->vertexPointers[j], obs->triIdxPointers[j], obs->transforms[j]))
		{
			collide_results[threadId] = COLLISION_RES;
			return;
		}
	}
	
	collide_results[threadId] = COLLISIONFREE_RES;
}


template <class StackElem, int StackFullDefault, class CollideFunctor>
__global__ void collideSamples3T(GRobot* robot, GObstacles* obs, float* samples, unsigned int nSamples, unsigned int* collide_results, CollideFunctor collideF)
{
	unsigned int threadId = (blockIdx.y * gridDim.x + blockIdx.x) * (blockDim.x * blockDim.y) + threadIdx.y * blockDim.x + threadIdx.x;
	if(threadId >= nSamples)
		return;
		
	GTransform t;
	
	t.ToTrans(samples[threadId], samples[threadId + nSamples], samples[threadId + 2 * nSamples]);
	
	for(unsigned int j = 0; j < obs->n; ++j)
	{
		if(collideF(robot->models[0], robot->vertexPointers[0], robot->triIdxPointers[0], t, obs->models[j], obs->vertexPointers[j], obs->triIdxPointers[j], obs->transforms[j]))
		{
			collide_results[threadId] = COLLISION_RES;
			return;
		}
	}
	
	collide_results[threadId] = COLLISIONFREE_RES;
}


template <class StackElem, int StackFullDefault, class CollideFunctor>
__global__ void collideSamplesArticulatedJoint(GRobot* robot, GObstacles* obs, GArticulatedStruct* artistru, float* samples, unsigned int nSamples, unsigned int joint_id, unsigned int* collide_results, CollideFunctor collideF)
{
	extern __shared__ char array[];
	unsigned short joint_num = robot->n;
	short* shared_joint_types = (short*)array;
	short* shared_param_id = (short*)&shared_joint_types[joint_num];
	short* shared_parent = (short*)&shared_param_id[joint_num];

	unsigned int threadId = (blockIdx.y * gridDim.x + blockIdx.x) * (blockDim.x * blockDim.y) + threadIdx.y * blockDim.x + threadIdx.x;
	if(threadId >= nSamples)
		return;


	if(threadIdx.x == 0)
	{
		for(unsigned short i = 0; i < joint_num; ++i)
		{
			shared_joint_types[i] = artistru->joint_type[i];
			shared_param_id[i] = artistru->param_id[i];
			shared_parent[i] = artistru->parent[i];
		}
	}

	__syncthreads();
		
	if(collide_results[threadId] == COLLISION_RES) return;
	
	short param_id;
	GTransform t, tmp;
	
	tmp = artistru->T0[joint_id];
	if(shared_joint_types[joint_id] == JT_CONTINUOUS || shared_joint_types[joint_id] == JT_REVOLUTE)
	{
		param_id = shared_param_id[joint_id];
		tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[joint_id]);
	}
	else if(shared_joint_types[joint_id] == JT_PRISMATIC)
	{
		param_id = shared_param_id[joint_id];
		tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[joint_id]);
	}
	
	t = tmp;
	short parent_id = shared_parent[joint_id];
	
	while(parent_id != ROBOT_NOPARENT_NODE)
	{
		tmp = artistru->T0[parent_id];
		if(shared_joint_types[parent_id] == JT_CONTINUOUS || shared_joint_types[parent_id] == JT_REVOLUTE)
		{
			param_id = shared_param_id[parent_id];
			tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
		}
		else if(shared_joint_types[parent_id] == JT_PRISMATIC)
		{
			param_id = shared_param_id[parent_id];
			tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
		}
		
		t.append_left(tmp);
		
		parent_id = shared_parent[parent_id];
	}
	
	
	for(unsigned int j = 0; j < obs->n; ++j)
	{
		if(collideF(robot->models[joint_id], robot->vertexPointers[joint_id], robot->triIdxPointers[joint_id], t, obs->models[j], obs->vertexPointers[j], obs->triIdxPointers[j], obs->transforms[j]))
		{
			collide_results[threadId] = COLLISION_RES;
			return;
		}
	}
}

template <class StackElem, int StackFullDefault, class CollideFunctor>
void collideSamplesArticulated_separate(GRobot* robot, unsigned int num_joints, GObstacles* obs, GArticulatedStruct* artistru, float* samples, unsigned int nSamples, unsigned int* collide_results, CollideFunctor collideF)
{
	GRobot h_robot;
	FROMGPU(&h_robot, robot, sizeof(GRobot));
	OBBNode** h_models = new OBBNode* [num_joints];
	FROMGPU(h_models, h_robot.models, sizeof(OBBNode*) * num_joints);
	
	dim3 grids = makeGrid((int)ceilf(nSamples / (float)COLLISION_THREADS));
	dim3 threads = dim3(COLLISION_THREADS, 1, 1);
	
	initMemory<unsigned int> <<< grids, threads>>>(collide_results, nSamples, COLLISIONFREE_RES);

	unsigned int shared_memory_size = 3 * sizeof(short) * num_joints;
	
	for(int i = 0; i < num_joints; ++i)
	{
		if(h_models[i])
			collideSamplesArticulatedJoint<StackElem, StackFullDefault, CollideFunctor> <<< grids, threads, shared_memory_size>>>(robot, obs, artistru, samples, nSamples, i, collide_results, collideF);
	}
	
	delete [] h_models;
	
}


template <class StackElem, int StackFullDefault, class CollideFunctor>
__global__ void collideSamplesArticulated_allInOne_PR2(GRobot* robot, GObstacles* obs, GArticulatedStruct* artistru, float* samples, GTransform* transforms, unsigned int nSamples, unsigned int* collide_results, CollideFunctor collideF)
{
	extern __shared__ char array[];
	unsigned short joint_num = robot->n;
	unsigned short obs_num = obs->n;
	unsigned short nNoSelfcollisionPairs = robot->nNoSelfcollisionPairs;

	unsigned int threadId = (blockIdx.y * gridDim.x + blockIdx.x) * (blockDim.x * blockDim.y) + threadIdx.y * blockDim.x + threadIdx.x;
	if(threadId >= nSamples)
		return;

	GTransform t, t2;

	for(unsigned short i = 0; i < joint_num; ++i)
	{
		if(robot->models[i])
		{
			t = transforms[threadId + i * nSamples];
			for(unsigned short j = 0; j < obs_num; ++j)
			{
				if(collideF(robot->models[i], robot->vertexPointers[i], robot->triIdxPointers[i], t, obs->models[j], obs->vertexPointers[j], obs->triIdxPointers[j], obs->transforms[j]))
				{
					collide_results[threadId] = COLLISION_RES;
					return;
				}
			}



			// self collision with other joints
			for(unsigned short j = i + 1; j < joint_num; ++j)
			{
				if(robot->models[j])
				{
					bool find = false;
					for(unsigned short k = 0; k < nNoSelfcollisionPairs; ++k)
					{
						ushort2 noCollisionPair = robot->noSelfcollisionPairs[k];
						if((noCollisionPair.x == i && noCollisionPair.y == j) || (noCollisionPair.y == i &&  noCollisionPair.x == j))
						{
							find = true; // no self collision
							break;
						}
					}

					if(find) continue;

					t2 = transforms[threadId + j * nSamples];

					if(collideF(robot->models[i], robot->vertexPointers[i], robot->triIdxPointers[i], t, robot->models[j], robot->vertexPointers[j], robot->triIdxPointers[j], t2))
					{
						collide_results[threadId] = COLLISION_RES;
						return;
					}
				}
			}
		}
	}

	collide_results[threadId] = COLLISIONFREE_RES;
}


template <class StackElem, int StackFullDefault, class CollideFunctor>
__global__ void collideSamplesArticulated_allInOne(GRobot* robot, GObstacles* obs, GArticulatedStruct* artistru, float* samples, unsigned int nSamples, unsigned int* collide_results, CollideFunctor collideF)
{
	extern __shared__ char array[];
	unsigned short joint_num = robot->n;
	unsigned short obs_num = obs->n;
	unsigned short nNoSelfcollisionPairs = robot->nNoSelfcollisionPairs;
	short* shared_joint_types = (short*)array;
	short* shared_param_id = (short*)&shared_joint_types[joint_num];
	short* shared_parent = (short*)&shared_param_id[joint_num];
	
	unsigned int threadId = (blockIdx.y * gridDim.x + blockIdx.x) * (blockDim.x * blockDim.y) + threadIdx.y * blockDim.x + threadIdx.x;
	if(threadId >= nSamples)
		return;
		
	if(threadIdx.x == 0)
	{
		for(unsigned short i = 0; i < joint_num; ++i)
		{
			shared_joint_types[i] = artistru->joint_type[i];
			shared_param_id[i] = artistru->param_id[i];
			shared_parent[i] = artistru->parent[i];
		}
	}
	
	__syncthreads();
	
	GTransform t, t2, tmp;
	short param_id;
	short joint_type;
	short parent_id;
	
	for(unsigned short i = 0; i < joint_num; ++i)
	{
		if(robot->models[i])
		{
			tmp = artistru->T0[i];
			joint_type = shared_joint_types[i];
			if(joint_type == JT_REVOLUTE || joint_type == JT_CONTINUOUS)
			{
				param_id = shared_param_id[i];
				tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[i]);
			}
			else if(joint_type == JT_PRISMATIC)
			{
				param_id = shared_param_id[i];
				tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[i]);
			}
			
			t = tmp;
			parent_id = shared_parent[i];
			
			while(parent_id != ROBOT_NOPARENT_NODE)
			{
				tmp = artistru->T0[parent_id];
				joint_type = shared_joint_types[parent_id];
				if(joint_type == JT_CONTINUOUS || joint_type == JT_REVOLUTE)
				{
					param_id = shared_param_id[parent_id];
					tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
				}
				else if(joint_type == JT_PRISMATIC)
				{
					param_id = shared_param_id[parent_id];
					tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
				}
				
				t.append_left(tmp);
				
				parent_id = shared_parent[parent_id];
			}
			
			for(unsigned short j = 0; j < obs_num; ++j)
			{
				if(collideF(robot->models[i], robot->vertexPointers[i], robot->triIdxPointers[i], t, obs->models[j], obs->vertexPointers[j], obs->triIdxPointers[j], obs->transforms[j]))
				{
					collide_results[threadId] = COLLISION_RES;
					return;
				}
			}
			
			
			// self collision with other joints
			for(unsigned short j = i + 1; j < joint_num; ++j)
			{
				if(robot->models[j])
				{
					bool find = false;
					for(unsigned short k = 0; k < nNoSelfcollisionPairs; ++k)
					{
						ushort2 noCollisionPair = robot->noSelfcollisionPairs[k];
						if((noCollisionPair.x == i && noCollisionPair.y == j) || (noCollisionPair.y == i &&  noCollisionPair.x == j))
						{
							find = true; // no self collision
							break;
						}
					}
					
					if(find) continue;
					
					
					tmp = artistru->T0[j];
					joint_type = shared_joint_types[j];
					if(joint_type == JT_REVOLUTE || joint_type == JT_CONTINUOUS)
					{
						param_id = shared_param_id[j];
						tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[j]);
					}
					else if(joint_type == JT_PRISMATIC)
					{
						param_id = shared_param_id[j];
						tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[j]);
					}
					
					t2 = tmp;
					parent_id = shared_parent[j];
					
					while(parent_id != ROBOT_NOPARENT_NODE)
					{
						tmp = artistru->T0[parent_id];
						joint_type = shared_joint_types[parent_id];
						if(joint_type == JT_CONTINUOUS || joint_type == JT_REVOLUTE)
						{
							param_id = shared_param_id[parent_id];
							tmp.append_rotate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
						}
						else if(joint_type == JT_PRISMATIC)
						{
							param_id = shared_param_id[parent_id];
							tmp.append_translate(samples[threadId + param_id * nSamples], artistru->axis[parent_id]);
						}
						
						t2.append_left(tmp);
						
						parent_id = shared_parent[parent_id];
					}
					
					if(collideF(robot->models[i], robot->vertexPointers[i], robot->triIdxPointers[i], t, robot->models[j], robot->vertexPointers[j], robot->triIdxPointers[j], t2))
					{
						collide_results[threadId] = COLLISION_RES;
						return;
					}
				}
			}
		}
	}
	
	collide_results[threadId] = COLLISIONFREE_RES;
}

template <int StackFullDefault>
void collideSamplesPR2(EnvironmentBase* penv, GRobot* robot, GObstacles* obs, GArticulatedStruct* artistruct, float* samples, unsigned int nSamples, unsigned int* collide_results)
{
	//unsigned int dim = penv->m_robot->dim_state;
	dim3 grids = makeGrid((int)ceilf(nSamples / (float)COLLISION_THREADS));
	dim3 threads = dim3(COLLISION_THREADS, 1, 1);

	unsigned int maxNTris_robot = 0;
	unsigned int maxNTris_obs = 0;

	for(int i = 0; i < penv->m_num_models; ++i)
	{
		if(penv->m_models[i]->modelType == ROBOT_MODEL)
		{
			if(maxNTris_robot < penv->m_models[i]->nTris)
				maxNTris_robot = penv->m_models[i]->nTris;
		}
		else if(penv->m_models[i]->modelType == OBSTACLE_MODEL)
		{
			if(maxNTris_obs < penv->m_models[i]->nTris)
				maxNTris_obs = penv->m_models[i]->nTris;
		}
	}

	switch(penv->m_robot->robot_type)
	{
	case RT_ARTICULATED:
		{
			unsigned int joint_num = penv->m_robot->joint_num;
			//unsigned int dim = penv->m_robot->dim_state;
			unsigned int nSelfCollisionPairs = ((ArticulatedRobot*)penv->m_robot)->noSelfcollision_info.size();
			unsigned int shared_memory_size = joint_num * (sizeof(short) * 3);

			unsigned int transform_shared_memory_size = joint_num * (sizeof(short) * 3);

			GTransform* transforms = NULL;
			GPUMALLOC((void**)&transforms, sizeof(GTransform) * nSamples);
			computeTransformArticulated<<<grids, threads, transform_shared_memory_size>>> (samples, artistruct, transforms, nSamples, joint_num);
			
			cudaThreadSynchronize();

#if COLLISION_MODE == ALLINONE_MODE
			if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
			{
				BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
				collideSamplesArticulated_allInOne_PR2<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples, transforms, nSamples, collide_results, collideF);
			}
			else
			{
				BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
				collideSamplesArticulated_allInOne_PR2<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples, transforms, nSamples, collide_results, collideF);
			}
#else //SEPARATE_MODE
			if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
			{
				BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
				collideSamplesArticulated_separate<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
			}
			else
			{
				BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
				collideSamplesArticulated_separate<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
			}
#endif
			GPUFREE(transforms);
		}
		break;

	}
}

template <int StackFullDefault>
void collideSamples1(EnvironmentBase* penv, GRobot* robot, GObstacles* obs, GArticulatedStruct* artistruct, float* samples, unsigned int nSamples, unsigned int* collide_results)
{
	//unsigned int dim = penv->m_robot->dim_state;
	dim3 grids = makeGrid((int)ceilf(nSamples / (float)COLLISION_THREADS));
	dim3 threads = dim3(COLLISION_THREADS, 1, 1);
	
	unsigned int maxNTris_robot = 0;
	unsigned int maxNTris_obs = 0;
	
	for(int i = 0; i < penv->m_num_models; ++i)
	{
		if(penv->m_models[i]->modelType == ROBOT_MODEL)
		{
			if(maxNTris_robot < penv->m_models[i]->nTris)
				maxNTris_robot = penv->m_models[i]->nTris;
		}
		else if(penv->m_models[i]->modelType == OBSTACLE_MODEL)
		{
			if(maxNTris_obs < penv->m_models[i]->nTris)
				maxNTris_obs = penv->m_models[i]->nTris;
		}
	}
	
	switch(penv->m_robot->robot_type)
	{
	case RT_ARTICULATED:
	{
		unsigned int joint_num = penv->m_robot->joint_num;
		//unsigned int dim = penv->m_robot->dim_state;
		unsigned int nSelfCollisionPairs = ((ArticulatedRobot*)penv->m_robot)->noSelfcollision_info.size();
		unsigned int shared_memory_size = joint_num * (sizeof(short) * 3);


#if COLLISION_MODE == ALLINONE_MODE
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamplesArticulated_allInOne<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamplesArticulated_allInOne<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples,  nSamples, collide_results, collideF);
		}
#else //SEPARATE_MODE
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamplesArticulated_separate<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamplesArticulated_separate<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
#endif
	}
	break;
	case RT_RIGID_3T3R:
	{
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamples3T3R<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamples3T3R<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
	}
	break;
	case RT_RIGID_3T:
	{
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollideFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamples3T<ushort2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollideFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamples3T<uint2, StackFullDefault, BVHCollideFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
	}
	break;
	default:
		;
	}
}

template <int StackFullDefault>
void collideSamples_packet(EnvironmentBase* penv, GRobot* robot, GObstacles* obs, GArticulatedStruct* artistruct, float* samples, unsigned int nSamples, unsigned int* collide_results)
{
	printf("packet used\n");
	//unsigned int dim = penv->m_robot->dim_state;
	dim3 grids = makeGrid((int)ceilf(nSamples / (float)(BVH_PACKET_BLOCK_WIDTH * BVH_PACKET_BLOCK_HEIGHT)));
	dim3 threads = dim3(BVH_PACKET_BLOCK_WIDTH, BVH_PACKET_BLOCK_HEIGHT, 1);
	
	unsigned int maxNTris_robot = 0;
	unsigned int maxNTris_obs = 0;
	
	for(int i = 0; i < penv->m_num_models; ++i)
	{
		if(penv->m_models[i]->modelType == ROBOT_MODEL)
		{
			if(maxNTris_robot < penv->m_models[i]->nTris)
				maxNTris_robot = penv->m_models[i]->nTris;
		}
		else if(penv->m_models[i]->modelType == OBSTACLE_MODEL)
		{
			if(maxNTris_obs < penv->m_models[i]->nTris)
				maxNTris_obs = penv->m_models[i]->nTris;
		}
	}
	
	switch(penv->m_robot->robot_type)
	{
	case RT_ARTICULATED:
	{
		unsigned int joint_num = penv->m_robot->joint_num;
		//unsigned int dim = penv->m_robot->dim_state;
		unsigned int shared_memory_size = joint_num * (sizeof(short) + sizeof(short));
#if COLLISION_MODE == ALLINONE_MODE
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamplesArticulated_allInOne<ushort2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamplesArticulated_allInOne<uint2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads, shared_memory_size>>>(robot, obs, artistruct, samples,  nSamples, collide_results, collideF);
		}
#else //SEPARATE_MODE
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamplesArticulated_separate<ushort2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamplesArticulated_separate<uint2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> >(robot, penv->m_robot->dim_state, obs, artistruct, samples, nSamples, collide_results, collideF);
		}
#endif
	}
	break;
	case RT_RIGID_3T3R:
	{
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamples3T3R<ushort2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamples3T3R<uint2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
	}
	break;
	case RT_RIGID_3T:
	{
		if(maxNTris_robot < 65535 && maxNTris_obs < 65535)
		{
			BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> collideF;
			collideSamples3T<ushort2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, ushort2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
		else
		{
			BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> collideF;
			collideSamples3T<uint2, StackFullDefault, BVHCollidePacketFunctor<OBBNode, OBB, uint2, 1> > <<< grids, threads>>>(robot, obs, samples, nSamples, collide_results, collideF);
		}
	}
	break;
	default:
		;
	}
}

#endif